clc
clear
close all

%% inicializace

Ts=0.1;
Tref=30;
Tmax=19;

refPomoc=zeros(2,Tref/Ts);

refPomoc(1,(Tref/0.4):(3*Tref/0.4))=-80;
refPomoc(2,(Tref/0.2):(Tref/0.1))=-80;

for i=1:length(refPomoc);
    ref(2*i-1)=refPomoc(1,i);
    ref(2*i)=refPomoc(2,i);
end


param.ref=ref;

%% simulace1

param.const=0; %koeficient pro matici H

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;

figure
hold on
plot(t,ref(1,:),'r',t,ref(2,:),'k');
plot(t,y(1,:),t,y(2,:),'g');

%% simulace2
param.const=1000;

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;


plot(t,y(1,:),'--',t,y(2,:),'g--');

%% simulace3
param.const=1000000;

[y,t,out]=simulace(@dualInt,@mpc_dualInt_soft,Tmax/Ts,Ts,zeros(2,1),zeros(2,1),param);

ref=out.ref;


plot(t,y(1,:),':',t,y(2,:),'g:');
grid on
legend('ref1','ref2','y1-0','y2-0','y1-1000','y2-1000','y1-1000000','y2-1000000');
ylabel('y [-]');
xlabel('t [s]');
title(['soft constranit ']);
